#include "Rp2Transformation.h"

using namespace Rp2;

//---------------------------------------------------------------------------------------------------
Transformation::Transformation()
	:
	m_kMatrix(1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f),
	m_kTranslate(0.0f,0.0f,0.0f),
	m_kScale(1.0f,1.0f,1.0f)
{
	// default transform is the identity
	m_bIsIdentity = true;
	m_bIsRSMatrix = true;
	m_bIsUniformScale = true;
}
//---------------------------------------------------------------------------------------------------
Transformation::~Transformation()
{

}
//---------------------------------------------------------------------------------------------------
void Transformation::MakeIdentity()
{
	m_kMatrix = Matrix3f::IDENTITY;
	m_kTranslate = Vector3f::ZERO;
	m_kScale = Vector3f(1.0f, 1.0f, 1.0f);
	m_bIsIdentity = true;
}
//---------------------------------------------------------------------------------------------------
void Transformation::MakeUnitScale()
{
	m_kScale = Vector3f(1.0f, 1.0f, 1.0f);
}
//---------------------------------------------------------------------------------------------------
void Transformation::SetTranslate(const Vector3f& rkTranslate)
{
	m_kTranslate = rkTranslate;
	m_bIsIdentity = false;
}
//---------------------------------------------------------------------------------------------------
void Transformation::SetRotate(const Matrix3f& rkRotate)
{
	m_kMatrix = rkRotate;
	m_bIsIdentity = false;
	m_bIsRSMatrix = true;
}
//---------------------------------------------------------------------------------------------------
void Transformation::SetScale(const Vector3f rkScale)
{
	m_kScale = rkScale;
	m_bIsIdentity = false;
	m_bIsUniformScale = false;
}
//---------------------------------------------------------------------------------------------------
void Transformation::SetUniformScale(float fScale)
{
	assert(m_bIsRSMatrix && fScale != 0.0f);
	m_kScale = Vector3f(fScale, fScale, fScale);
	m_bIsIdentity = false;
	m_bIsUniformScale = true;
}
//---------------------------------------------------------------------------------------------------
void Transformation::SetMatrix(const Matrix3f& rkMatrix)
{
	m_kMatrix = rkMatrix;
	m_bIsIdentity = false;
	m_bIsRSMatrix = false;
	m_bIsUniformScale = false;
}
//---------------------------------------------------------------------------------------------------
float Transformation::GetNorm () const
{
    if (m_bIsRSMatrix)
    {
        float fMax = Mathf::FAbs(m_kScale.X());
        if (Mathf::FAbs(m_kScale.Y()) > fMax)
        {
            fMax = Mathf::FAbs(m_kScale.Y());
        }
        if (Mathf::FAbs(m_kScale.Z()) > fMax)
        {
            fMax = Mathf::FAbs(m_kScale.Z());
        }
        return fMax;
    }

    // A general matrix.  Use the max-row-sum matrix norm.  The spectral
    // norm (the maximum absolute value of the eigenvalues) is smaller or
    // equal to this norm.  Therefore, this function returns an approximation
    // to the maximum scale.
    float fMaxRowSum =
        Mathf::FAbs(m_kMatrix[0]) +
        Mathf::FAbs(m_kMatrix[1]) +
        Mathf::FAbs(m_kMatrix[2]);

    float fRowSum =
        Mathf::FAbs(m_kMatrix[3]) +
        Mathf::FAbs(m_kMatrix[4]) +
        Mathf::FAbs(m_kMatrix[5]);

    if ( fRowSum > fMaxRowSum )
        fMaxRowSum = fRowSum;

    fRowSum =
        Mathf::FAbs(m_kMatrix[6]) +
        Mathf::FAbs(m_kMatrix[7]) +
        Mathf::FAbs(m_kMatrix[8]);

    if (fRowSum > fMaxRowSum)
    {
        fMaxRowSum = fRowSum;
    }

    return fMaxRowSum;
}
//---------------------------------------------------------------------------------------------------
void Transformation::Product(const Transformation& rkA, const Transformation& rkB)
{
	if (rkA.IsIdentity())
	{
		*this = rkB;
		return;
	}

	if (rkB.IsIdentity())
	{
		*this = rkA;
		return;
	}

	if (rkA.m_bIsRSMatrix && rkB.m_bIsRSMatrix)
	{
		if (rkA.m_bIsUniformScale)
		{
			SetRotate(rkA.m_kMatrix * rkB.m_kMatrix);

			SetTranslate(rkA.GetUniformScale() * (
				rkA.m_kMatrix * rkB.m_kTranslate) + rkA.m_kTranslate);

			if (rkB.m_bIsUniformScale)
			{
				SetUniformScale(rkA.GetUniformScale() * rkB.GetUniformScale());
			}
			else
			{
				SetScale(rkA.GetUniformScale() * rkB.GetScale());
			}

			return;
		}
	}

    // In all remaining cases, the matrix cannot be written as R*S*X+T.
	Matrix3f kMA = (rkA.m_bIsRSMatrix ? 
		rkA.m_kMatrix.DiagonalTimes(rkA.m_kScale) :
		rkA.m_kMatrix);

	Matrix3f kMB = (rkB.m_bIsRSMatrix ?
		rkB.m_kMatrix.DiagonalTimes(rkB.m_kScale) :
		rkB.m_kMatrix);

	SetMatrix(kMA * kMB);
	SetTranslate(kMA * rkB.m_kTranslate + rkA.m_kTranslate);
}
//---------------------------------------------------------------------------------------------------
Vector3f Transformation::ApplyForward (const Vector3f& rkInput) const
{
    if (m_bIsIdentity)
    {
        // Y = X
        return rkInput;
    }

    if (m_bIsRSMatrix)
    {
        // Y = R*S*X + T
        Vector3f kOutput(m_kScale.X()*rkInput.X(),m_kScale.Y()*rkInput.Y(),
            m_kScale.Z()*rkInput.Z());
        kOutput = kOutput * m_kMatrix + m_kTranslate;
        return kOutput;
    }

    // Y = M*X + T
    Vector3f kOutput = rkInput * m_kMatrix + m_kTranslate;
    return kOutput;
}
//---------------------------------------------------------------------------------------------------
void Transformation::ApplyForward (int iQuantity, const Vector3f* akInput,
    Vector3f* akOutput) const
{
    if (m_bIsIdentity)
    {
        // Y = X
        size_t uiSize = iQuantity*sizeof(Vector3f);
        System::Memcpy(akOutput,uiSize,akInput,uiSize);
    }
    else
    {
        int i;
        if (m_bIsRSMatrix)
        {
            // Y = R*S*X + T
            for (i = 0; i < iQuantity; i++)
            {
                akOutput[i].X() = m_kScale.X()*akInput[i].X();
                akOutput[i].Y() = m_kScale.Y()*akInput[i].Y();
                akOutput[i].Z() = m_kScale.Z()*akInput[i].Z();
                akOutput[i] = akOutput[i] * m_kMatrix + m_kTranslate;
            }
        }
        else
        {
            // Y = M*X + T
            for (i = 0; i < iQuantity; i++)
            {
                akOutput[i] = akInput[i] * m_kMatrix + m_kTranslate;
            }
        }
    }
}
//---------------------------------------------------------------------------------------------------
Vector3f Transformation::ApplyInverse (const Vector3f& rkInput) const
{
    if (m_bIsIdentity)
    {
        // X = Y
        return rkInput;
    }

    Vector3f kOutput = rkInput - m_kTranslate;
    if (m_bIsRSMatrix)
    {
        // X = S^{-1}*R^t*(Y - T)
        kOutput = m_kMatrix * kOutput;
        if (m_bIsUniformScale)
        {
            kOutput /= GetUniformScale();
        }
        else
        {
            // The direct inverse scaling is
            //   kOutput.X() /= m_kScale.X();
            //   kOutput.Y() /= m_kScale.Y();
            //   kOutput.Z() /= m_kScale.Z();
            // When division is much more expensive than multiplication,
            // three divisions are replaced by one division and ten
            // multiplications.
            float fSXY = m_kScale.X()*m_kScale.Y();
            float fSXZ = m_kScale.X()*m_kScale.Z();
            float fSYZ = m_kScale.Y()*m_kScale.Z();
            float fInvDet = 1.0f/(fSXY*m_kScale.Z());
            kOutput.X() *= fInvDet*fSYZ;
            kOutput.Y() *= fInvDet*fSXZ;
            kOutput.Z() *= fInvDet*fSXY;
        }
    }
    else
    {
        // X = M^{-1}*(Y - T)
        kOutput = kOutput * m_kMatrix.Inverse();
    }

    return kOutput;
}
//----------------------------------------------------------------------------
void Transformation::ApplyInverse (int iQuantity, const Vector3f* akInput,
    Vector3f* akOutput) const
{
    if (m_bIsIdentity)
    {
        // X = Y
        size_t uiSize = iQuantity*sizeof(Vector3f);
        System::Memcpy(akOutput,uiSize,akInput,uiSize);
        return;
    }

    Vector3f kDiff;
    int i;
    if (m_bIsRSMatrix)
    {
        // X = S^{-1}*R^t*(Y - T)
        if (m_bIsUniformScale)
        {
            float fInvScale = 1.0f/GetUniformScale();
            for (i = 0; i < iQuantity; i++)
            {
                kDiff = akInput[i] - m_kTranslate;
                akOutput[i] = fInvScale * (m_kMatrix * kDiff);
            }
        }
        else
        {
            // The direct inverse scaling is
            //   fInvXScale = 1.0f/m_kScale.X();
            //   fInvYScale = 1.0f/m_kScale.Y();
            //   fInvZScale = 1.0f/m_kScale.Z();
            // When division is much more expensive than multiplication, three
            // divisions are replaced by one division and ten multiplications.
            float fSXY = m_kScale.X()*m_kScale.Y();
            float fSXZ = m_kScale.X()*m_kScale.Z();
            float fSYZ = m_kScale.Y()*m_kScale.Z();
            float fInvDet = 1.0f/(fSXY*m_kScale.Z());
            float fInvXScale = fInvDet*fSYZ;
            float fInvYScale = fInvDet*fSXZ;
            float fInvZScale = fInvDet*fSXY;
            for (i = 0; i < iQuantity; i++)
            {
                kDiff = akInput[i] - m_kTranslate;
                akOutput[i] = m_kMatrix * kDiff;
                akOutput[i].X() *= fInvXScale;
                akOutput[i].Y() *= fInvYScale;
                akOutput[i].Z() *= fInvZScale;
            }
        }
    }
    else
    {
        // X = M^{-1}*(Y - T)
        Matrix3f kInverse = m_kMatrix.Inverse();
        for (i = 0; i < iQuantity; i++)
        {
            kDiff = akInput[i] - m_kTranslate;
            akOutput[i] = kDiff * kInverse;
        }
    }
}

//---------------------------------------------------------------------------------------------------
Vector3f Transformation::ApplyForwardVector (const Vector3f& rkInput) const
{
    if (m_bIsIdentity)
    {
        // X = Y
        return rkInput;
    }

    if (m_bIsRSMatrix)
    {
        // Y = X*S*R
        Vector3f kOutput(m_kScale.X()*rkInput.X(),m_kScale.Y()*rkInput.Y(),
            m_kScale.Z()*rkInput.Z());
        kOutput = kOutput * m_kMatrix;
        return kOutput;
    }

    // Y = X * M
    Vector3f kOutput = rkInput * m_kMatrix;
    return kOutput;
}
//---------------------------------------------------------------------------------------------------
Vector3f Transformation::InvertVector (const Vector3f& rkInput) const
{
    if (m_bIsIdentity)
    {
        // X = Y
        return rkInput;
    }

    Vector3f kOutput;
    if (m_bIsRSMatrix)
    {
        // X = S^{-1}*R^t*Y
        kOutput = m_kMatrix * rkInput;
        if (m_bIsUniformScale)
        {
            kOutput /= GetUniformScale();
        }
        else
        {
            // The direct inverse scaling is
            //   kOutput.X() /= m_kScale.X();
            //   kOutput.Y() /= m_kScale.Y();
            //   kOutput.Z() /= m_kScale.Z();
            // When division is much more expensive than multiplication,
            // three divisions are replaced by one division and ten
            // multiplications.
            float fSXY = m_kScale.X()*m_kScale.Y();
            float fSXZ = m_kScale.X()*m_kScale.Z();
            float fSYZ = m_kScale.Y()*m_kScale.Z();
            float fInvDet = 1.0f/(fSXY*m_kScale.Z());
            kOutput.X() *= fInvDet*fSYZ;
            kOutput.Y() *= fInvDet*fSXZ;
            kOutput.Z() *= fInvDet*fSXY;
        }
    }
    else
    {
        // X = Y * M^{-1}
        kOutput = rkInput * m_kMatrix.Inverse();
    }

    return kOutput;
}
//---------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------
void Transformation::GetHomogeneous(Matrix4f& rkM) const
{
	if (m_bIsRSMatrix)
	{
		rkM[ 0] = m_kScale[0] * m_kMatrix[0];
		rkM[ 1] = m_kScale[0] * m_kMatrix[1];
		rkM[ 2] = m_kScale[0] * m_kMatrix[2];
		rkM[ 3] = 0.0f;
		rkM[ 4] = m_kScale[1] * m_kMatrix[3];
		rkM[ 5] = m_kScale[1] * m_kMatrix[4];
		rkM[ 6] = m_kScale[1] * m_kMatrix[5];
		rkM[ 7] = 0.0f;
		rkM[ 8] = m_kScale[2] * m_kMatrix[6];
		rkM[ 9] = m_kScale[2] * m_kMatrix[7];
		rkM[10] = m_kScale[2] * m_kMatrix[8];
		rkM[11] = 0.0f;
	}
	else
	{
		rkM[ 0] = m_kMatrix[0];
		rkM[ 1] = m_kMatrix[1];
		rkM[ 2] = m_kMatrix[2];
		rkM[ 3] = 0.0f;
		rkM[ 4] = m_kMatrix[3];
		rkM[ 5] = m_kMatrix[4];
		rkM[ 6] = m_kMatrix[5];
		rkM[ 7] = 0.0f;
		rkM[ 8] = m_kMatrix[6];
		rkM[ 9] = m_kMatrix[7];
		rkM[10] = m_kMatrix[8];
		rkM[11] = 0.0f;
	}

	rkM[12] = m_kTranslate[0];
	rkM[13] = m_kTranslate[1];
	rkM[14] = m_kTranslate[2];
	rkM[15] = 1.0f;

}
//---------------------------------------------------------------------------------------------------